from calm.actuator import Actuator
from system.eventServer import eventServer

import math

class MovementActuator(Actuator):
    def __init__(self, controller, vars):
        Actuator.__init__(self, controller, vars, 3)
        
        self.move = 0
        self.turn = 0
        
    def acceptBits(self, bits, dtime):
        self.move = bits[0]
        
        if bits[1] != bits[2]:
            if bits[1]:
                self.turn = -1
            else:
                self.turn = 1
        else:
            self.turn = 0
            
        speed = self.agent.getVar("speed")
        turnSpeed = self.agent.getVar("turnSpeed")
        
        oldx, oldy = x,y = self.agent.getVar("pos")
        rot = self.agent.getVar("rot")
        
        rot += self.turn * turnSpeed * dtime
        
        newx = x + self.move * speed * dtime * math.cos(rot)
        newy = y + self.move * speed * dtime * math.sin(rot)
        
        #if self.move:
        #    print math.sqrt((newx-x)**2 + (newy-y)**2), speed, self.move
        
        #print self.move, self.turn, self.move * speed * dtime, self.turn * turnSpeed * dtime
        #print speed, turnSpeed, dtime
        
        self.agent.setVar("pos",(newx,newy))
        self.agent.setVar("rot",rot)
        
        #print bits, self.move, self.turn
        
